#include "Util.h"

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <iostream>

#include <cmath>

using namespace boost::numeric::ublas;

namespace cgmath 
{
	template<class T>
	vector<T> cross(vector<T> & u,
		vector<T> & v) 
	{
		vector<T> result(3);
		result(0) = u(1)*v(2)-u(2)*v(1);
		result(1) = u(2)*v(0)-u(0)*v(2);
		result(2) = u(0)*v(1)-u(1)*v(0);

		return result;
	}

	template<class T>
	T deg2Rad(T angle) 
	{
		return (T)((angle/360)*2.0f*PI);
	}

	template<class T>
	T rad2Deg(T angle) 
	{
		return (T)(angle/(2.0f*PI))*360.0f;
	}

	template<class T>
	T signedPlaneDist(vector<T> & p0,
					  vector<T> & n,
					  vector<T> & point) {
		T norm = norm_2(n);
		return (T)inner_prod(n,point-p0)/norm;
	}

	template<class T>
	matrix<T> rotXMat(T angle) 
	{
		using namespace std;

		angle = deg2Rad(angle);

		matrix<T> rot = identity_matrix<T>(3,3);

		rot(1,1) = cos(angle);
		rot(1,2) = -sin(angle);
		rot(2,1) = sin(angle);
		rot(2,2) = cos(angle);

		return rot;
	}


	template<class T>
	matrix<T> rotYMat(T angle) 
	{
		using namespace std;

		angle = deg2Rad(angle);

		matrix<T> rot = identity_matrix<T>(3,3);

		rot(0,0) = cos(angle);
		rot(0,2) = sin(angle);
		rot(2,0) = -sin(angle);
		rot(2,2) = cos(angle);

		return rot;
	}


	template<class T>
	matrix<T> rotZMat(T angle) 
	{
		using namespace std;

		angle = deg2Rad(angle);

		matrix<T> rot = identity_matrix<T>(3,3);

		rot(0,0) = cos(angle);
		rot(0,1) = -sin(angle);
		rot(1,0) = cos(angle);

		return rot;
	}


	template<class T>
	matrix<T> rotMatrix(vector<T> & r,
		vector<T> & s,
		vector<T> & t,
		T delta) 
	{      
		matrix<T> M(3,3);

		row(M,0) = r;
		row(M,1) = s;
		row(M,2) = t;

		matrix<T> Rx(3,3);
		Rx(0,0) = 1; Rx(0,1) = 0;               Rx(0,2) = 0;
		Rx(1,0) = 0; Rx(1,1) = std::cos(delta); Rx(1,2) = -std::sin(delta);
		Rx(2,0) = 0; Rx(2,1) = std::sin(delta); Rx(2,2) = std::cos(delta);

		matrix<T> tmp = prod(trans(M),Rx);

		return prod(tmp,M);
	}

	template<class T>
	void arrayToVector(T * data, vector<T> & target) {
		for(unsigned int i = 0; i <target.size(); i++) {
			target(i) = data[i];
		}
	}

	template<class T>
	void arrayToMatrix(T * data, matrix<T> & target) {
		for(unsigned int r = 0; r < target.size1(); r++) {
			for(unsigned int c = 0; c < target.size2(); c++) {
				target(c,r) = data[r*target.size2()+c];
			}
		}
	}

	template<class T>
	void matrixToArray(T * data,
		matrix<T> & target) 
	{
		data[0] = target(0,0);
		data[1] = target(1,0);
		data[2] = target(2,0);
		data[3] = target(3,0);
		data[4] = target(0,1);
		data[5] = target(1,1);
		data[6] = target(2,1);
		data[7] = target(3,1);
		data[8] = target(0,2);
		data[9] = target(1,2);
		data[10] = target(2,2);
		data[11] = target(3,2);
		data[12] = target(0,3);
		data[13] = target(1,3);
		data[14] = target(2,3);
		data[15] = target(3,3);
	}

	template<class T>
	void quaternionToMatrix(const boost::math::quaternion<T> & q, 
		matrix<T> & matrix) 
	{
		T W = q.R_component_1();
		T X = q.R_component_2();
		T Y = q.R_component_3();
		T Z = q.R_component_4();

		T xx      = X * X;
		T xy      = X * Y;
		T xz      = X * Z;
		T xw      = X * W;

		T yy      = Y * Y;
		T yz      = Y * Z;
		T yw      = Y * W;

		T zz      = Z * Z;
		T zw      = Z * W;

		matrix(0,0)  = 1 - 2 * ( yy + zz );
		matrix(0,1)  =     2 * ( xy - zw );
		matrix(0,2)  =     2 * ( xz + yw );

		matrix(1,0)  =     2 * ( xy + zw );
		matrix(1,1)  = 1 - 2 * ( xx + zz );
		matrix(1,2)  =     2 * ( yz - xw );

		matrix(2,0)  =     2 * ( xz - yw );
		matrix(2,1)  =     2 * ( yz + xw );
		matrix(2,2)  = 1 - 2 * ( xx + yy );

		matrix(0,3) = matrix(1,3) = matrix(2,3) = matrix(3,0) = matrix(3,1) = matrix(3,2) = 0;
		matrix(3,3) = 1;
	}

	template<class T>
	void matrixToQuaternion(const matrix<T> & matrix,
		boost::math::quaternion<T> & q) 
	{

		// compute trace
		T trace = matrix(0,0) + matrix(1,1) + matrix(2,2)+1;

		T s,w,x,y,z;

		// if trace greater than 0
		if(trace > 0) {
			std::cout << "T gt 0" << std::endl;

			s = 0.5f / (T)std::sqrt(trace);
			w = 0.25f / s;
			x = (matrix(2,1) - matrix(1,2))*s;
			y = (matrix(0,2) - matrix(2,0))*s;
			z = (matrix(1,0) - matrix(0,1))*s;
		}
		else {
			if(matrix(0,0) > matrix(1,1)&&
				matrix(0,0) > matrix(2,2)) {
					std::cout << "0,0 greatest" << std::endl;

					s = (T)std::sqrt( 1.0f + matrix(0,0) - matrix(1,1) - matrix(2,2) ) * 2.0f;

					x = 0.5f / s;
					y = (matrix(0,1) + matrix(1,0) ) / s;
					z = (matrix(0,2) + matrix(2,0) ) / s;
					w = (matrix(1,2) + matrix(2,1) ) / s;

			}
			else if(matrix(1,1) > matrix(0,0)&&
				matrix(1,1) > matrix(2,2)) {
					std::cout << "1,1 greatest" << std::endl;

					s  = (T)std::sqrt( 1.0 + matrix(1,1) - matrix(0,0) - matrix(2,2) ) * 2;

					x = (matrix(0,1) + matrix(1,0) ) / s;
					y = 0.5f / s;
					z = (matrix(1,2) + matrix(2,1) ) / s;
					w = (matrix(0,2) + matrix(2,0) ) / s;
			}  
			else {
				std::cout << "2,2 greatest" << std::endl;

				s  = (T)std::sqrt( 1.0 + matrix(2,2) - matrix(0,0) - matrix(1,1) ) * 2;

				x = (matrix(0,2) + matrix(2,0) ) / s;
				y = (matrix(1,2) + matrix(2,1) ) / s;
				z = 0.5f / s;
				w = (matrix(0,1) + matrix(1,0) ) / s;
			}

		}
		boost::math::quaternion<T> q1(w,x,y,z);
		q = q1;
	}


	template<class T>
	void EulerAngles(const matrix<T> & matrix,
		T & h,
		T & r,
		T & p) 
	{ 
		using namespace std;

		if(matrix(2,1)>0) {
			if(matrix(2,2)>=0.0) {
				p = asin(matrix(2,1));
			}
			else {
				p = (T)(PI-asin(matrix(2,1)));
			}
		}
		else {
			if(matrix(2,2)>=0.0) {
				p = asin(matrix(2,1));
			}
			else {
				p = (T)-(PI+asin(matrix(2,1)));
			}
		}

		if(fabs(cos(p))<0.01) {
			h = 0;
			r = atan2(matrix(1,0),matrix(0,0));
		}
		else {
			if(fabs(p)<PI/2) {
				h = atan2(-matrix(2,0),matrix(2,2));
				r = atan2(-matrix(0,1),matrix(1,1));
			}
			else {
				h = (T)(atan2(-matrix(2,0),matrix(2,2))+PI);
				r = (T)(atan2(-matrix(0,1),matrix(1,1))+PI);
			}
		}
	}

	template<class T>
	void matrixFromAngles(matrix<T>& matrix,
		const T & h,
		const T & r,
		const T & p) 
	{
		using namespace std;

		matrix(0,0) = cos(r)*cos(h)-sin(r)*sin(p)*sin(h);
		matrix(0,1) = -sin(r)*cos(p);
		matrix(0,2) = cos(r)*sin(h)+sin(r)*sin(p)*cos(h);

		matrix(1,0) = sin(r)*cos(h)+cos(r)*sin(p)*sin(h);
		matrix(1,1) = cos(r)*cos(p);
		matrix(1,2) = sin(r)*sin(h)-cos(r)*sin(p)*cos(h);

		matrix(2,0) = -cos(p)*sin(h);
		matrix(2,1) = sin(p);
		matrix(2,2) = cos(p)*cos(h);
	}  

	template<class T>
	void translationFromMatrix(const matrix<T> & matrix,
		T& trX,
		T& trY,
		T& trZ) 
	{
		trX = matrix(0,3);
		trY = matrix(1,3);
		trZ = matrix(2,3);
	}

	template<class T>
	void translationToMatrix(matrix<T> & matrix,
		const T& trX,
		const T& trY,
		const T& trZ) 
	{
		matrix(0,3) = trX;
		matrix(1,3) = trY;
		matrix(2,3) = trZ;
	}

	template<class T>
	void printMatrix(const matrix<T> & matrix) 
	{
		for(unsigned int r = 0; r < matrix.size1(); r++) {
			for(unsigned int c = 0; c < matrix.size2(); c++) {
				std::cout <<  matrix(c,r) << " ";
			}
			std::cout << std:: endl;
		}
	}

	template<class T>
	T bernstein(unsigned int n, unsigned int i, T t) 
	{
		return binom(n,i)*std::pow((T)t,(T)i)*std::pow((T)1-t,(T)n-i);
	}


	unsigned int binom(unsigned int n, unsigned int m) 
	{
		std::vector<unsigned int> b(n+1);
		b[0] = 1;
		for(unsigned int i = 1; i <=n; ++i) {
			b[i] = 1;

			for(unsigned int j = i-1U; j>0; --j) {
				b[j] += b[j - 1U];
			}

		}
		return (unsigned int)b[m];
	}

	// init templates:

	template float deg2Rad(float angle);
	template double deg2Rad(double angle);

	template float rad2Deg(float angle);
	template double rad2Deg(double angle);

	template float signedPlaneDist(vector<float> & p0, vector<float> & n, vector<float> & point);
	template double signedPlaneDist(vector<double> & p0, vector<double> & n, vector<double> & point);

	template matrix<float> rotZMat(float angle);
	template matrix<double> rotZMat(double angle);

	template matrix<float> rotYMat(float angle);
	template matrix<double> rotYMat(double angle);

	template matrix<float> rotXMat(float angle);
	template matrix<double> rotXMat(double angle);

	template vector<float> cross(vector<float> & u,
		vector<float> & v);
	template vector<double> cross(vector<double> & u,
		vector<double> & v);

	template matrix<float> rotMatrix(vector<float> & r,
		vector<float> & s,
		vector<float> & t,
		float delta);

	template matrix<double> rotMatrix(vector<double> & r,
		vector<double> & s,
		vector<double> & t,
		double delta);

	template void arrayToVector(float * data, vector<float> & target);
	template void arrayToVector(double * data, vector<double> & target);

	template void arrayToMatrix(float * data, matrix<float> & target);
	template void arrayToMatrix(double * data, matrix<double> & target);

	template void matrixToArray(float * data, matrix<float> & target);
	template void matrixToArray(double * data, matrix<double> & target);

	template void quaternionToMatrix(const boost::math::quaternion<float> & q, 
		matrix<float> & targetMatrix);
	template void quaternionToMatrix(const boost::math::quaternion<double> & q, 
		matrix<double> & targetMatrix);

	template void matrixToQuaternion(const matrix<float> & matrix,
		boost::math::quaternion<float> & q);

	template void matrixToQuaternion(const matrix<double> & matrix,
		boost::math::quaternion<double> & q);

	template void EulerAngles(const matrix<float> & matrix,
		float & h,
		float & r,
		float & p);
	template void EulerAngles(const matrix<double> & matrix,
		double & h,
		double & r,
		double & p);

	template void translationFromMatrix(const matrix<float> & matrix, float& trX,float& trY,float& trZ);  
	template void translationFromMatrix(const matrix<double> & matrix, double& trX,double& trY,double& trZ);  

	template void translationToMatrix(matrix<float> & matrix,const float& trX,const float& trY,const float& trZ);
	template void translationToMatrix(matrix<double> & matrix,const double& trX,const double& trY,const double& trZ);

	template void matrixFromAngles(matrix<float>& matrix,
		const float& h,
		const float& r,
		const float& p);
	template void matrixFromAngles(matrix<double>& matrix,
		const double& h,
		const double& r,
		const double& p);

	template void printMatrix(const matrix<float> & matrix);
	template void printMatrix(const matrix<double> & matrix);

	template float bernstein(unsigned int n, unsigned int i, float t);
	template double bernstein(unsigned int n, unsigned int i, double t);
}
